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1 Problem Review 


In order for the EVAHR robot to autonomously track and grasp objects, its vision system must be able 
to determine the 3-D structure and motion of an object from a sequence of sensory images. This task 
is accomplished by the use of a laser radar range sensor which provides dense range maps of the scene. 
Unfortunately, the currently available laser radar range cameras use a sequential scanning approach which 
complicates image analysis. Although many algorithms have been developed for recognizing objects from 
range images, none are suited for use with single beam, scanning, time-of-flight sensors because all previous 
algorithms assume instantaneous acquisition of the entire image. This assumption is invalid since the EVAHR 
robot is equipped with a sequential scanning laser range sensor. If an object is moving while being imaged 
by the device, the apparent structure of the object can be significantly distorted due to the significant 
non-zero delay time between sampling each image pixel. If an estimate of the motion of the object can 
be determined, this distortion can be eliminated; but, this leads to the motion-structure paradox - most 
existing algorithms for 3-D motion estimation use the structure of objects to parameterize their motions. 
The goal of this research is to design a rigid-body motion recovery technique which overcomes this limitation. 
The method being developed is an iterative, linear, feature-based approach which uses the non-zero image 
acquisition time constraint to accurately recover the motion parameters from the distorted structure of the 
3-D range maps. Once the motion parameters are determined, the structural distortion in the range images 
is corrected. 

2 Accomplishments 9/1/93 - 12/15/93 

In the initial phase of this research, the following issues were addressed: 

• The motion recovery algorithm was developed. 

• The behavior of the algorithm in the absence of correspondence error was verified using a computer 
simulation in which the correspondence between object features in successive image frames was known 
exactly. 

• The behavior of the algorithm in the presence of feature correspondence errors was studied using 
simulated laser radar imagery with correspondence performed by a human observer. 

• The stability and convergence of the iterative algorithm were studied and a formal, analytical proof of 
its stability was developed. 

• The above research resulted in the submission of a paper to the 1994 IEEE Conference on Computer 
Vision and Pattern Recognition . 

2.1 The Motion Recovery Algorithm 

• Initialize 

— Use the sequence of reflectance and range images to extract and establish correspondence between 
N object point pairs k = 0, 1 , . . N — 1. 

- Compute the time between sampling for the fcth pair of points, A for each value of k = 
0,1,..., A — 1 and calculate the average acquisition delay time, t av = -fa 

- Set Aa .,0 = 0 for each value of k = 0, 1, . . ., N — 1, and set the iteration counter m = 0. 

pfk f p k 

— Use the relation ~ Ro(£ at ,)^^ + T Uy0 to estimate (Ro(f a v)> T Ui0 ). 

• Loop 
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- Use (R TO (f a u),T u>m ) to estimate the velocity of the object, (0 m , $ m , t m , X m , Y m , Z m ). 

- Use the rotational velocity estimates to compute R m (A <*) and Afc, m , k = 0, 1, . . . , N — 1. 

p*. j , ph . p* . 

- Use the relation - Afc iTn ^ = R m +i + T Uj?n+ i to form a system of linear equations. 

Solve this system to refine the estimate of the motion transformation (R m+ i(f at; ), T u>m+1 ). 

— Let m = m+1 and repeat the loop until the change in any of the motion parameters is below 
some user defined threshold. 

• Structural Distortion Removal 

— For each point pij , determine its 3-D coordinate at time t = 0 using the relation 

v Pi j = R” 1 (^t)[pjj — T(iSt)] where 8t = T s (i + nj) and T s is the inter-pixel sampling time. 

2.2 Simulation with Exact Feature Correspondence 

In order to verify the behavior of the motion recovery algorithm without the effects of feature correspondence 
error, a computer simulation was developed using MATLAB and an IBM RS6000 workstation. The test data 
were generated by defining eight points on an object and eight different transformations based on the Euler 
angle representation of the rotation matrix. Each transformation was calculated using the same rotational 
and translational velocities, but different, randomly chosen sampling delay times, A^.. The rotational and 
translational velocities used in this test were randomly chosen to be 0 = 1.3 rads/sec, 4> = 0.9 rads/sec, 
T = —1.2 rads/sec, v x = 90 units/sec, v y = —80 units/sec, and v z = 30 units/sec.. Each point was then “set 
in motion” using its respective transformation as is shown in Figure 1(b). The result of these transformations 
thus produced the set while the original object points formed the set {p-j}. The iterative algorithm 

described above was applied to these data sets, and the resulting velocity estimates were then used to 
reconstruct the actual three dimensional structure of the object. 

As can be seen in Figure 3, the algorithm converged rapidly to the proper rotational and translational 
velocities after only a small number of iterations. Figure 2 shows the result of reconstructing the true 
structure of the object using the velocity estimates produced by the iterative technique. The structural 
distortion caused by the different times of rotation for each point has been eliminated. 

2.3 Experiment Using Simulated Laser Radar Images 

The iterative motion recovery algorithm was then tested using simulated laser radar images of an object 
moving against a background of constant depth. The purpose of these tests was to determine the effects 
of correspondence errors on the convergence of the algorithm. Figure 4 shows a two frame sequence of 
simulated laser range images (taken with a frame rate of 2 frames/sec) of four “reflectors” on the same 
object undergoing rigid body motion. Correspondence between the points was determined manually. The 
three dimensional points extracted from the images in Figures 4(a) and (b) were then used to form the sets 
{Pij} ^d respectively. The iterative motion recovery algorithm was then applied to these data sets, 

and the resulting velocity estimates were then used to reconstruct the actual three dimensional structure of 
the object. 

The actual rotational and translational velocities of the object and their estimates after a number of 
iterations are shown in Table 1. The convergence of the estimates is shown in Figure 5. While the velocity 
estimates still converge to a finite value, there is measurable error in the estimate of the velocities. These 
discrepancies between the estimated and the actual velocities are due to errors in determining point cor- 
respondence between the two range images. The version of the iterative algorithm employed in this test 
used a simple least squares algorithm to update the estimates of R m (t a v) and T u m at each iteration level, 
producing considerable cumulative error in each velocity estimate. If a more robust method for solving 
systems of linear equations is employed, the effect of these accumulated errors could be minimized further. 
Note, however, that the iterative algorithm provided more accurate measures of the object’s velocity than 
the initial estimate provided. 
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2.4 Stability of the Iterative Estimation Algorithm 

In order for the algorithm described in Section 2 to provide reasonable estimates of the rotational and 
translational velocities of objects moving within the range field, the adaptive law governing the estimator 
must be stable in the sense of Lyapunov. Below two theorems are included which will be employed to prove 
that the iterative procedure is stable. 

Theorem 1 If matrices Qi, Q 2 6 C nXn are such that Q x = [Pi : P 2 ] and Q 2 = [P x j 0] where P x £ C nxm 
and P 2 € C ), then Q x Q 2 = diag(I mXm , 0( n _ TO , x ( n _ m ,) where I TOXm ** the m x m identity matrix. 

Proof. 


Q 2 = [Pi : 0] 

= [Pi : P 2 ] 


b>»xm 0 mx(n _ m) 

L ®(n— m)xm ®(n-rn)x(n— »»i 

_ Qj I Hmx(n-m) 

[ 0(n— m)xm 0(n-rn)x(n— m) 


and thus, 


Qr ! Q 2 = 


Q.E.D. 


I»n xm Omx(n-m) 

^(n-m)xm 0(n-m) X (n- m ) 

Theorem 2 The equilibrium state x—0 of a linear, time-invariant, discrete, time system of the form x[Jb+l] = 
E*[*] + F «[*] w here E € C nXn , F £ C nXm , x[k] £ K n , and u[A] £ TZ m , is stable if and only if the spectral 
radius |/>(E)| < 1 and every eigenvalue of E with a magnitude of one is a simple zero of the minimum 
polynomial o/E. 

Proof: the proof is omitted here because it is beyond the scope of this report and can be found in almost 
any graduate level text in modern systems control. 

In order to use theorem 2 to determine the stability of the iterative motion recovery process, the algo- 
rithm’s adaptive law must be expressed in the form of a linear, time invariant discrete time system. The 

re ation = R m +i(£ at ,)^^ +T Uim _j_i used to update the estimate of the motion transformation 

in the iteration loop can be rewritten to yield 


p'ly - = R a [m];4 + Ai t T u [m]. 


( 1 ) 


let n ?. R >] " Afc[m] = A k>m , and T„[m] = T u>m , where m represents 

the mth iteration step. This equation produces three equations in twelve unknowns, so a set of at least four 
three dimensional points must be used to solve for /J 0 [m]. Thus, 


“ P*j' 


" Ai[m] 

0 

0 

0 

P?J‘ 


0 

A 2 [m] 

0 

0 



0 

0 

A 3 [m] 

0 

L Pi', j* J 


0 

0 

0 

A 4 [m] 


R a [m] 

0 

0 

0 


0 

R a [m] 

0 

0 


0 

0 

Ra[ra] 

0 


■ 

“ Pij ' 

r 


Ph 

_3 

+ 


p i J 



L Pij J 

L 


T u [m] 

0 

0 

0 


0 

T u [rn] 
0 
0 


0 

0 

T u [m] 
0 


can be rewritten in terms of the unknown elements of R a [m] and T„[m] to yield 

p' - A[m]p = Wjffin] 

where 


o 
o 
0 

T u [m] 


T 

' At, ‘ 


Ah 


Ah 

J 

At+ 


(2) 


( 3 ) 
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P' = [ (p'lyf (Ph'f (p'lyf (rh>) T ] T 

A[m] = diagfAi [m], A 2 [m], A 3 [m], A 4 [m]) 

P=[«>) T (Phf (Phf (Phf V 


. W, 


r (p 1 ) T 
0 
0 

tfJ) T 

0 

0 

(P 3 ) r 
o 
o 

( P i ) T 
0 
0 


0 

(pi ) T 

«> 

0 

0 

(p? ) T 
0 


«> 

•i- 

0 


«> 


Aej 

o 

0 

At 2 

0 

0 

At 3 

0 

0 

Af 4 

0 

0 


0 

At A 

0 

0 

At2 

0 

0 

At 3 

0 

0 

A *4 
0 


0 

0 

Ati 

0 

0 

A t2 
0 
0 

At 3 

0 

0 

At 4 J 


• r[m] — [ rn[m] ri2[m] ri3[m] r2i[m] r 2 2[m] r23[ni] r 3 i[m] r32[m] r3 3 [m] ti[m] ^[m] f3[m] ] T where 

represents the (i, j)th element of R a [m] and fi[m] represents the fth element of T u [m]. 

However, since A/.[m] = Rjt[m - 1] — R a [m — 1], the matrix A[rn] can be expressed as A[m] = — 

1] — Ra[™]Ii 2 xi 2 where R^[m — 1] — diag(Ri[m - 1], R- 2 [?n — 1], R 3 [m — 1], R 4 [m — 1]). Thus, by substitution 
in equation 3 


P' - (R*[w» - 1] - Rah - I]li2xi2 )P = Wjr[m] 
p' - R fc [m - 1];7+ R a [m - l]I 12x i 2 p = Wjr[m] 
p — Rjt[m — l]p + W 2 f[m — 1] = Wjffin]. 


(4) 


where 


W 2 = 


(p* V 
0 
0 

<P?,y> r 

o 

0 

(p 3 ) T 

o 

0 


0 


0 

0 


0 

0 

0 

0 

(pi ) T 


A slight rearrangement of equation 4 yields the following linear, time invariant discrete difference equation 
in terms of the unknown parameter vector, f[m], 


r[m + 1] = Er[?n] + Fu[7n] (5) 

where 

E = W-’Wj (6) 

F = [-W,- 1 ; Wf'] (7) 

«[»»] = [ffc[m] T p' T ] r (8) 

r k [m] = R^.[7n - 1 ]p. (9) 
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By careful observation of the definitions of W x and W 2 , it is noted that they are of the form 


W x = [P, i P 2 ], W 2 = [Pj : 0]. 

Thus, theorem 1 can be used to show that 

E = W" 1 W 2 = diag(I 9 x 9 , 0 3 X 3 ) (10) 

So, since E is an idempotent matrix, the spectrum, cr( E) 6 {0, 1} and the spectral radius, |/>(E)| < 1. 
Thus, since the eigenvalue Ai = 1 is a simple zero of E and |p(E)| < 1, the iterative motion recovery method 
presented in the previous subsection is stable in the sense of Lyapunov as per theorem 2. The estimate of 
the rotational and translational parameters is therefore always finite, since the input vector u[m] is always 
bounded due to the fact that p, p* are constant vectors and R^[m — 1] is a unitary matrix. 

3 Future Work 

In the coming year, the following theoretical and experimental issues will be considered. First, the proposed 
algorithm will be tested using real data sets taken from the EVA HR’s laser mapping sensors as soon as the 
system is again operational. Segmentation and correspondence will, at first, be done manually so that severe 
point mismatches won’t have to be considered in the initial algorithm development. After analyzing the 
algorithm’s performance on real data, the continued research will focus on the theoretical proof of convergence 
for the motion transformation parameters using the iterative approach described. From the analytical study 
and the experimentation with the actual EVAHR data sets, the conditions under which this algorithm can 
be used will be more concretely determined. Before the system is implemented, however, the computational 
aspects of the algorithm, including methods of reducing the computational complexity while increasing the 
accuracy of estimating the motion transformation, the numerical behavior of the various matrices, and 
the error tolerance in the initial estimation will be examined thoroughly. After these preliminary studies, 
segmentation and correspondence procedures for determining the necessary sets of point correspondence 
pairs for the motion estimation algorithm will be designed and developed in order to make the motion 
estimation procedure completely autonomous. An approximate schedule of the work is shown below. 

Proposed Schedule for 1/1/94 - 12/31/94 

• Spring 1994 - Test the algorithm using real datasets obtained from the EVAHR’s laser ranging system. 
Continue to develop a proof of convergence , and begin to study the numerical/computational issues 
for efficient and robust implementation of the motion estimation procedure. 

• Summer 1994 - Visit NASA JSC, and continue to study numerical/implementation issues. Begin 
development of automatic correspondence techniques. 

• Fall 1994 - Continue development of the automatic correspondence technique and begin to implement 
a complete system. 
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Object Positions Prior to and After Transformation by t_avg Object Positions Prior to and After Transformation by delaljr 




(a) (b) 

Figure 1: (a) the original object and its pose after each point is transformed using a relation based 
on tavgi (b) the original object and its pose after each point is transformed using its own relation 
based on A 


Object Position Reconstructed by the Algorithm 



Figure 2: Object structure reconstructed using the velocity estimates generated by the iterative 
algorithm. The structural distortion present in the transformed object has been eliminated. 
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(a) 


image 2 for Test 1 
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(b) 


Figure 4: Laser radar range image sequence of four blocks moving with same velocity 



Actual Velocity 

Estimated Velocity 

Error 

v x (m/sec) 

-7 

-7.14 

.14 

v y (m/sec) 

15 

15 

0 

v z (m/sec) 

-3 

-2.64 

.36 

0 (rads/sec) 

0 

0 

0 

T (rads/sec) 

0 

0 

0 

$ (rads /sec) 

.2 

.2 

0 


Table 1: A comparison of the actual velocity and estimated velocity determined by the iterative 
algorithm for the simulated laser range images. 
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